#include "DataPack.h"
#include <SensorUtility/sensorutility_global.h>

DataPack::DataPack (const QImage& originImg, const cv::Mat& recognizedCorners,
		cv::Size boardSize) : _origin (originImg),
		_recognizedCorners (recognizedCorners), _boardSize (boardSize)
{
	_genRecognizeResult ();
}

DataPack::DataPack ()
{

}

DataPack::~DataPack ()
{

}

void DataPack::genValidationImages (const cv::Mat& intrinsicMat,
		const cv::Mat& undistortCeofs,
		const cv::Mat& rVec, const cv::Mat& tVec, const cv::Mat& objectPoints)
{
// 	_undistort =  QImage (_origin.size (), QImage::Format_RGB32);
 	_reprojected = _origin.copy ();
	cv::Mat originCvImg (_origin.height (), _origin.width (),
			qImageFormat2CvImageType (_reprojected.format ()),
			(void*)_origin.constBits ());
// 	cv::Mat undistortCvImg (_undistort.height (), _undistort.width (), CV_32FC4,
// 			_undistort.bits ());
	cv::Mat reprojectedCvImg (_reprojected.height (), _reprojected.width (),
			qImageFormat2CvImageType (_reprojected.format ()),
			_reprojected.bits ());
	cv::Mat projectedPoints;
 	cv::undistort (originCvImg, reprojectedCvImg, intrinsicMat, undistortCeofs);
	cv::projectPoints (objectPoints, rVec, tVec, intrinsicMat,
			cv::Mat::zeros (1, 5, CV_64FC1), projectedPoints);
	cv::drawChessboardCorners (reprojectedCvImg, _boardSize,
			projectedPoints, true);
}

void DataPack::_genRecognizeResult ()
{
	_recognizeResult = _origin.copy ();
	cv::Mat recognizeResultCvImg (
			_recognizeResult.height (), _recognizeResult.width (),
			qImageFormat2CvImageType (_recognizeResult.format ()),
			(void*)_recognizeResult.constBits ());
	cv::drawChessboardCorners (recognizeResultCvImg, _boardSize,
			_recognizedCorners, true);
}

const cv::Mat& DataPack::getCorners () const
{
	return _recognizedCorners;
}

const QImage& DataPack::getOrigin () const
{
	return _origin;
}

const QImage& DataPack::getRecognizeResult () const
{
	return _recognizeResult;
}

const QImage& DataPack::getUndistort () const
{
	return _undistort;
}

const QImage& DataPack::getReproject () const
{
	return _reprojected;
}
